#include "include/VectorTest.h"
#include "include/App.h"
#include "include/Scripting.h"
#include "include/MagAcc.h"
#include "include/KalmanFilters.h"

using namespace Ei;

MagAcc::MagAcc()
{
	
	// 	
	// load user filter settings settings
	//
	loadSettings();
	
	mVeloFilter[X].init();
	mVeloFilter[Y].init();
	mVeloFilter[Z].init();
	
	mMagNoiseFilter[X].init();
	mMagNoiseFilter[Y].init();
	mMagNoiseFilter[Z].init();
	
	mMagMoveFilter[X].init();
	mMagMoveFilter[Y].init();
	mMagMoveFilter[Z].init();
	
	mAccNoiseFilter[X].init();
	mAccNoiseFilter[Y].init();
	mAccNoiseFilter[Z].init();
	
	resetFilters(0);// reset mag filters
	resetFilters(1);// reset acc filters
	resetFilters(2);// reset acc filters
	// set bools
	mCalibrated = false;
	useFilter =  0;
	
	// add debug vectors
	mDebugVectors["mindata"] = Vector3(0,0,0);
	mDebugVectors["maxdata"] = Vector3(0,0,0);
	mDebugVectors["rawmagdata"] = Vector3(0,0,0);
	mDebugVectors["rawaccdata"] = Vector3(0,0,0);
	mDebugVectors["oldaccdata"] = Vector3(0,0,0);
	mDebugVectors["interpolatedMagData"] = Vector3(0,0,0);
	mDebugVectors["oldNorth"] = Vector3(0,0,0);
	mDebugVectors["oldGravity"] = Vector3(0,-1.63725,0);
	mDebugVectors["oldintergratedGravity"] = Vector3(0,-1.63725,0);
	mDebugVectors["dt"] = Vector3(0,0,0);
	
	mIntergratedGravity = Vector3(0,0,0);
	setupVectorObjects();

}

MagAcc::~MagAcc()
{

}
////////////////////////////////////////
//  Private functions //////////////////
///////////////////////////////////////

void MagAcc::splitGravityAndAcceleration(Vector3 data)
{

	// use kalmanFilter to split Velocity and gravity
	mAccNoiseFilter[X].Measure(data.x);
   	mAccNoiseFilter[Y].Measure(data.y);
  	mAccNoiseFilter[Z].Measure(data.z);
   		
        mGravity.x = mAccNoiseFilter[X].Predict();
        mGravity.y = mAccNoiseFilter[Y].Predict();
        mGravity.z = mAccNoiseFilter[Z].Predict();
        
       
	// intergrate data to get a more stable down vector
        Real f = mSettings["acc_interstep"];
	Real g = mSettings["acc_interamount"];
	mIntergratedGravity.x = (mGravity.x /f) + ( g * mIntergratedGravity.x);
	mIntergratedGravity.y = (mGravity.y /f) + ( g * mIntergratedGravity.y);
	mIntergratedGravity.z = (mGravity.z /f) + ( g * mIntergratedGravity.z);
	
	

	mDebugVectors["oldIntergratedGravity"] = mIntergratedGravity;
     
	// accelaration is rawdata - gravity
	mAcceleration = data - mIntergratedGravity;	
	mVectorObjects["mAccFiltert"]->setScale(mGravity);	
	mVectorObjects["mAccGravity"]->setScale(mIntergratedGravity);
	mVectorObjects["mAccMeter"]->setScale(data);
	mVectorObjects["mAccAcceleration"]->setScale(mAcceleration);
 

   	
}
/////////////////////////////////////////
//////////// used by netmanager//////////
/////////////////////////////////////////


void MagAcc::insertMagData(Vector3 newData)
{

   // add Raw data
   mDebugVectors["rawmagdata"] = newData * 0.0001;
   //rawMagData.normalise();
  // App::app()->mDebugString01 = "Mag data " + StringConverter::toString(newData);
   mVectorObjects["mMagMeter"]->setScale(mDebugVectors["rawmagdata"]);
   
   // Measure New data for my VeloFilter
   mVeloFilter[X].Measure(mDebugVectors["rawmagdata"].x);
   mVeloFilter[Y].Measure(mDebugVectors["rawmagdata"].y);
   mVeloFilter[Z].Measure(mDebugVectors["rawmagdata"].z);
   	
}

void MagAcc::insertAccData(Vector3 newData)
{
   
   // Split Acceleration and Gravity
   splitGravityAndAcceleration(newData * 0.0001);
	   
   // make current old acc data
    mDebugVectors["oldaccdata"] = newData * 0.0001;
}

/////////////////////////////////////////
//////////// used by App/////////////////
/////////////////////////////////////////
void MagAcc::update(float dt)
{
   /// Predict Magnetometer
   mDebugVectors["interpolatedMagData"].x =  mVeloFilter[X].Predict();
   mDebugVectors["interpolatedMagData"].y =  mVeloFilter[Y].Predict();
   mDebugVectors["interpolatedMagData"].z =  mVeloFilter[Z].Predict();	

   mMagNoiseFilter[X].Measure(mDebugVectors["interpolatedMagData"].x);
   mMagNoiseFilter[Y].Measure(mDebugVectors["interpolatedMagData"].y);
   mMagNoiseFilter[Z].Measure(mDebugVectors["interpolatedMagData"].z);
   
   mNorth.x = mMagNoiseFilter[0].Predict();
   mNorth.y = mMagNoiseFilter[1].Predict();
   mNorth.z = mMagNoiseFilter[2].Predict();
  
   mDebugVectors["dt"] = mNorth - mDebugVectors["interpolatedMagData"];
  
   static bool movingFilterset = false;
   if (mDebugVectors["dt"].squaredLength() > mSettings["magstation_switch"])
   {	
 	
	   	mMagNoiseFilter[X].dt = 1.0;
	  	mMagNoiseFilter[X].reinit();
   	 	mMagNoiseFilter[Y].dt = 1.0;
   	 	mMagNoiseFilter[Y].reinit();
         	mMagNoiseFilter[Z].dt = 1.0;	
	 	mMagNoiseFilter[Z].reinit();
	  	movingFilterset = true;	
	  	mVectorObjects["mOrientationMag"]->setColour(1);	
	 		
   }
   else
   {
	
	  	mMagNoiseFilter[X].dt= mSettings["magstation_dt"];
	  	mMagNoiseFilter[X].reinit();
   	  	mMagNoiseFilter[Y].dt = mSettings["magstation_dt"];
   	  	mMagNoiseFilter[Y].reinit();
          	mMagNoiseFilter[Z].dt = mSettings["magstation_dt"];	
	  	mMagNoiseFilter[Z].reinit();
	  	movingFilterset = false;	
	  	mVectorObjects["mOrientationMag"]->setColour(2);  
	 
	 
   }
   
   mVectorObjects["mMagInter"]->setScale(mDebugVectors["interpolatedMagData"]);
   mVectorObjects["mMagFilter"]->setScale(mNorth);
   mVectorObjects["mMagFilter"]->setColour(2);

}

void MagAcc::calibrateSensor()
{
	// calculate the yaw using the north vector
 	// and a vector pointing down.	
 	Vector3 down =  Vector3(0,-1,0);
 	Vector3 north = mNorth;
 	// only normalise here
	north.normalise(); 

	Vector3 downNorthCross = (north).crossProduct(down);
	Vector3 mUp    = down ;
	Vector3 mRight = downNorthCross;
	Vector3 mFront = ( mUp.crossProduct(mRight) );
	mUp = mUp * -1;
	
	Radian yaw = Quaternion(mRight,mUp,mFront).getYaw();
	
	// calculate the pitch and roll using the gravity vector
 	// an a front vector
 	
 	Vector3 front =  Vector3(-1,0,0);
 	Vector3 gravity =  mIntergratedGravity;
 	// only normalise here
	gravity.normalise();
	
	Vector3 frontGravityCross = (front).crossProduct(gravity);
	
	mUp    = gravity ;
	mRight = frontGravityCross;
	mFront = ( mUp.crossProduct(mRight) );
	mUp = mUp * -1;
	
	// get pitch and roll
	Quaternion quat = Quaternion(mRight,mUp,mFront);
	Radian pitch =  quat.getPitch(); 
	Radian roll  =  quat.getRoll(); 
 	
    	// contruct the current orientatopn
	Matrix3 orientation;
	orientation.FromEulerAnglesXYZ(yaw,pitch,roll);
	filterOffset = Quaternion(orientation);	
	
	// show debug bollen
	
	
	
	mCalibrated = true;		
}


Quaternion MagAcc::getOrientation()
{
   if (mCalibrated)
	{

 	Vector3 front = mNorth;
 	Vector3 gravity =  mIntergratedGravity;
 	// only normalise here
	gravity.normalise();
	front.normalise();
	
	Vector3 frontGravityCross = (front).crossProduct(gravity);
	
	Vector3 mUp    = gravity ;
	Vector3 mRight = frontGravityCross;
	Vector3 mFront = ( mUp.crossProduct(mRight) );
	mUp = mUp * -1;
	
	mUp.normalise();
	mRight.normalise();
	mFront.normalise();
	
	// get pitch and roll
	Quaternion quat = Quaternion(mRight,mUp,mFront);

	
	//eilog("pitch :" + StringConverter::toString(pitch.valueDegrees()));
	
 	
 	App::sceneMgr()->getSceneNode("Frontbol")->setPosition((mFront * 6) );	
	App::sceneMgr()->getSceneNode("Rightbol")->setPosition((mRight * 6));
	App::sceneMgr()->getSceneNode("Upbol")->setPosition((mUp * 6));
	
    	// contruct the current orientatopn
		
	mVectorObjects["mOrientationMag"]->setOrientation(quat);
	
	return quat;	
    }
    return Quaternion(1,0,0,0);				
}
////////////////////////////////////////////////////////
///// used by gui /////////////////////////////////////
//////////////////////////////////////////////////////
void MagAcc::loadSettings()
{
	// load magdata from file
	ConfigFile cf;
	cf.load("filters.cfg");	
	ConfigFile::SectionIterator seci = cf.getSectionIterator();

	while(seci.hasMoreElements()) {
		String secName = seci.peekNextKey();
		ConfigFile::SettingsMultiMap* settings = seci.getNext();
		ConfigFile::SettingsMultiMap::iterator i;
		 for (i = settings->begin(); i != settings->end(); ++i)
		 { 
			mSettings[secName + "_" + i->first] = StringConverter::parseReal(i->second);
			eilog("added " + secName + "_" + i->first +  ": number : " + i->second);					
		 }
	}
	
	
}
void MagAcc::resetFilters(int filterset)
{
	
	//insert uservalues in the filter
	
	if (filterset == 1) // load magnetometer data
	{
	
		for (int i =0; i< 3 ; i++)
		{
		
	//	mMagNoiseFilter[i].dt = mSettings["magstation_dt"];
		mMagNoiseFilter[i].V_NOISE = mSettings["magstation_V_NOISE"];  
		mMagNoiseFilter[i].V_GAMMA = mSettings["magstation_V_GAMMA"];  
		mMagNoiseFilter[i].OBS_NOISE = mSettings["magstation_OBS_NOISE"]; 
		mMagNoiseFilter[i].reinit();	
		}
	}
	if (filterset == 2)
	{
	
		for (int i =0; i< 3 ; i++)
		{
	
		
	//	mMagNoiseFilter[i].dt = mSettings["magstation_dt"];
		mMagNoiseFilter[i].V_NOISE = mSettings["magstation_V_NOISE"];  
		mMagNoiseFilter[i].V_GAMMA = mSettings["magstation_V_GAMMA"];  
		mMagNoiseFilter[i].OBS_NOISE = mSettings["magstation_OBS_NOISE"]; 
		mMagNoiseFilter[i].reinit();
		}
		
	}
	
	
	if (filterset == 3)
	{

		for (int i =0; i< 3 ; i++)
		{
		
		mAccNoiseFilter[i].dt = mSettings["acc_dt"];
		mAccNoiseFilter[i].V_NOISE = mSettings["acc_V_NOISE"];  
		mAccNoiseFilter[i].V_GAMMA = mSettings["acc_V_GAMMA"];  
		mAccNoiseFilter[i].OBS_NOISE = mSettings["acc_OBS_NOISE"]; 
		mAccNoiseFilter[i].reinit();
	
		}
	
	}
		
	
	

}
////////////////////////////////////////////////////////
////////// measure functions 
////////////////////////////////////////////////////////

void MagAcc::measureExtremes(Vector3 data)
{
	
	bool changed = false;
	if (mDebugVectors["maxdata"].x < data.x){
	    mDebugVectors["maxdata"].x = data.x; changed =true;}
	    
	if (mDebugVectors["maxdata"].y < data.y){
	    mDebugVectors["maxdata"].y = data.y; changed =true;}
	    	
	if (mDebugVectors["maxdata"].z < data.z){
	    mDebugVectors["maxdata"].z = data.z; changed =true;}
	    
	if (mDebugVectors["mindata"].x > data.x){
	    mDebugVectors["mindata"].x  = data.x; changed =true;}
	    
	if (mDebugVectors["mindata"].y > data.y){
	    mDebugVectors["mindata"].y  = data.y; changed =true;}
	    	
	if (mDebugVectors["mindata"].z > data.z){
	    mDebugVectors["mindata"].z = data.z; changed =true;}
	    
/*	if (changed){

	eilog ("Max " + StringConverter::toString(&mDebugVectors["mindata"]));
	eilog ("Min " + StringConverter::toString(&mDebugVectors["mindata"]));
	eilog ("//////////////////////////////////////////");
	}*/
}

void MagAcc::calculateOffset()
{
	
	eilog ("Xoffset " + StringConverter::toString(mDebugVectors["mindata"].x + mDebugVectors["maxdata"].x));
	eilog ("Yoffset " + StringConverter::toString(mDebugVectors["mindata"].y + mDebugVectors["maxdata"].y));
	eilog ("Zoffset " + StringConverter::toString(mDebugVectors["mindata"].z + mDebugVectors["maxdata"].z));
	eilog ("//////////////////////////////////////////");
	eilog ("XGain " + StringConverter::toString(Math::Abs(mDebugVectors["mindata"].x) + Math::Abs(mDebugVectors["maxdata"].x)));
	eilog ("YGain " + StringConverter::toString(Math::Abs(mDebugVectors["mindata"].y) + Math::Abs(mDebugVectors["maxdata"].y)));
	eilog ("ZGain " + StringConverter::toString(Math::Abs(mDebugVectors["mindata"].z) + Math::Abs(mDebugVectors["maxdata"].z)));
	eilog ("//////////////////////////////////////////");
}


///////////////////////////////////////////////////////////////////////////////////////
/////////// debug functions ///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////

void MagAcc::setupVectorObjects()
{
    	//
    	// Accelerometer objects
    	//
      	
    	mVectorObjects["mAccMeter"] = new Ei::VectorObject(Vector3(10,0,0));
        mVectorObjects["mAccFiltert"]  = new Ei::VectorObject(Vector3(10,0,.5));
        mVectorObjects["mAccGravity"]  = new Ei::VectorObject(Vector3(10,-2,10));
        mVectorObjects["mAccGravityFilter"]  = new Ei::VectorObject(Vector3(10,-2,10.5));
        mVectorObjects["mAccAcceleration"] = new Ei::VectorObject(Vector3(10,-3,15.5));
        mVectorObjects["mAccVelocity"]  = new Ei::VectorObject(Vector3(10,-3,15));
        
        mVectorObjects["mAccFiltert"]->setColour(1);
        mVectorObjects["mAccGravityFilter"]->setColour(2);
        mVectorObjects["mAccGravity"]->setColour(1);
        
        //
        // Magneto meter objects
        //
        
        mVectorObjects["mMagMeter"]  = new Ei::VectorObject(Vector3(-10,0,0));
        mVectorObjects["mMagInter"] = new Ei::VectorObject(Vector3(-10,-1,5));
        mVectorObjects["mMagFilter"] = new Ei::VectorObject(Vector3(-10,-1,5.5));
        
	mVectorObjects["mMagFilter"]->setColour(1); 
        mVectorObjects["mMagMeter"]->setColour(0);
        mVectorObjects["mMagInter"]->setColour(1);
        
        //
        // Orientation objects
        //
        
        mVectorObjects["mOrientationMag"] = new Ei::VectorObject(Vector3(0,0,-10));
        mVectorObjects["mOrientationObj"] = new Ei::VectorObject(Vector3(0,0,0));
        
	mVectorObjects["mOrientationMag"]->setScale(Vector3(.5,.5,.5));
        mVectorObjects["mOrientationObj"]->setScale(Vector3(.5,.5,.5));
   
}

int MagAcc::showVectors(int vecswitch)
{
  int MAXCASES = 3;	
  switch (vecswitch)
	 {
		case 0:
		{
		mVectorObjects["mAccFiltert"]->show(false);
		mVectorObjects["mAccGravity"]->show(false);
		mVectorObjects["mAccVelocity"]->show(false);
		break;
		}
		case 1:
		{
		mVectorObjects["mAccFiltert"]->show(true);
		break;
		}	
		case 2:
		{
		mVectorObjects["mAccGravity"]->show(true);
		break;
		}
		case 3:
		{
		mVectorObjects["mAccVelocity"]->show(true);
		break;
		}				
	 }
     vecswitch++;
     if (vecswitch > MAXCASES)
	 {vecswitch = 0;}
  	
     return vecswitch;	
	
}

/*	
void MagAcc::measureNoise(Vector3 data)
{
    //
    // calculate avarage over 200 samples, 
    // difference between avarage and data is noise;
      
    mAverageData.push_back(data);
	
   if (mAverageData.size() > 100)
    {
	mAverageData.pop_front();	
     }	
		
    // calculate avarge data	
    std::list<Ogre::Vector3>::iterator it;    
    Vector3 aData(0,0,0);
    for(it=mAverageData.begin(); it!=mAverageData.end(); ++it)
     {         
	 aData =  aData + ((*it) * (1.0f/mAverageData.size()));	
     } 	
    App::app()->mMagFilter->setScale(aData); 
     // calculate  noise from average data
     Vector3 currentNoise;
     currentNoise.x = Math::Abs(aData.x - data.x);
     currentNoise.y = Math::Abs(aData.y - data.y);
     currentNoise.z = Math::Abs(aData.z - data.z);
     
     mNoise.push_back(currentNoise);
    
    if (mNoise.size() > 30)
    {
	mNoise.pop_front();	
    }		
     
    Vector3 aNoise(0,0,0);
    for(it=mNoise.begin(); it!=mNoise.end(); ++it)
     {         
	 aNoise =  aNoise + ((*it) * (1.0f/mNoise.size()));	
     }

  //return (aData - data);

}
*/
